//
// Created by zhanglei on 2023/12/18.
//

#ifndef SLAM_IN_AUTO_DRIVING_SENSOR_CALIBRATION_H
#define SLAM_IN_AUTO_DRIVING_SENSOR_CALIBRATION_H

#include <fstream>
#include <functional>
#include <utility>

#include "common/gnss.h"
#include "common/imu.h"
#include "common/lidar_utils.h"

#include "common/math_utils.h"
#include "common/message_def.h"
#include "common/odom.h"
#include "common/eigen_types.h"
#include "ch3/utm_convert.h"

#include "common/read_data.h"

namespace sad {
// sensor center: vehicle
class SensorCalib {
public:
    SensorCalib();
    Eigen::Matrix4d ComputeImuPose();
    Eigen::Matrix4d ComputeGpsPose();
    Eigen::Matrix4d ComputeLeftVelodynePose();
    SE3 EigenToSE3(const Eigen::Matrix4d& pose);
    Eigen::Matrix4d SE3ToEigen(const SE3& pose);
    void PoseToRt(Eigen::Matrix3d& R, Eigen::Vector3d& t, const Eigen::Matrix4d& pose);
    Eigen::Matrix4d RtToPose(const Eigen::Matrix3d& R, const Eigen::Vector3d& t);

    Eigen::Matrix4d TransLidarToImu(const Eigen::Matrix4d& imu_pose, const Eigen::Matrix4d& lidar_pose);
    SE3 ImuToLidarMotion(const SE3& imu_pose, const Eigen::Matrix4d& lidar_pose);
    SE3 LidarToImuMotion(const SE3& imu_pose, const Eigen::Matrix4d& lidar_pose);

    Eigen::Matrix4d GetLeftVelodynePoseInImu() {
        return left_velodyne_pose_in_imu_;
    }
private:
    Eigen::Matrix4d imu_pose_in_vehicle_;
    Eigen::Matrix4d gps_pose_in_vehicle_;
    Eigen::Matrix4d left_velodyne_pose_in_vehicle_;
    Eigen::Matrix4d left_velodyne_pose_in_imu_;

};

}  // namespace sad

#endif  // SLAM_IN_AUTO_DRIVING_SENSOR_CALIBRATION_H
